;----------------------------------------------------------------------------
; FileName: Func.asm
; define base function used in image processing,such as 32bit divide
; tange calculate,recaltange etc.
; written by wang zhi gang
; date:   06/25/2000
;----------------------------------------------------------------------------
        .mmregs
        .include "fisc54.inc"
        .ref IMAGEADDR,Theta,DeltX,DeltY,Divisor,Minus
        .ref SqrtValue,Sqrt_temp,Sqrt_root,AtanTable,CosTable
		.ref SinValue,CosValue,DistanceY,DistanceX,CoordY
		.ref Dividen,Remain,Quoten,Theta3,Theta1,CoordX
        .def Sqroot,GetSinValue,GetCosValue,MultipyValue
		.def Cal_Coordinate,DivideU32,CombinePoint,Cal_Distance
        .def CalAngle,BoundFill,ReCalAngle,TangleValue,Check_Theta

*****************************************************************************
* MultipyValue: multipy two signed 16bit integer
*****************************************************************************
        .text
MultipyValue:
        ST  #0,Minus
        BC  MULTIPY_AMINUS,ALT
        BC  MULTIPY_VALUE,BGEQ    ;A>=0 && B>=0
        ST  #1,Minus              ;A>=0 && B<0
        B   MULTIPY_VALUE
MULTIPY_AMINUS
        BC  MULTIPY_VALUE,BLT     ;A<0 && B<0
        ST  #1,Minus              ;A<0 && B>=0      
MULTIPY_VALUE
        ABS  A
        ABS  B
        STLM A,T
        STL  B,Sqrt_temp
        LD   Sqrt_temp,16,A
        MPYA B
        CMPM Minus,#0
        RC   TC
        NEG  B
        RET

*****************************************************************************
* sqroot: calculate the square root of x
*****************************************************************************
        .text
Sqroot:
        ST    #0,Sqrt_root
        LD    SqrtValue,A
        STM   #7,BRC
        RPTB  Step_Loop
        LD    Sqrt_root,2,B
        ADD   #1,B
        STL   B,Sqrt_temp
        SUB   Sqrt_temp,14,A,B
        BC    No_SubData,BLT
        ADD   #0,B,A
        LD    Sqrt_root,B
        STL   B,1,Sqrt_root
        ORM   #01h,Sqrt_root
        B     Step_Loop
No_SubData
        LD    Sqrt_root,B
        BC    Step_Loop,BEQ
        STL   B,1,Sqrt_root
Step_Loop
        SFTA  A,2,A
        RET

*****************************************************************************
* fill image bound edge width black
*****************************************************************************
        .text
CombinePoint:
        MAR  *+AR6(-IMAGEWIDTH-1)
        LD   *AR6+0,B            ;lefttop point
        OR   B,-4,A
        LD   *AR6+0,B            ;left point
        OR   B,-3,A
        LD   *AR6+,B             ;rightbootom point
        OR   B,-2,A
        LD   *AR6+,B             ;bottom point
        OR   B,-1,A
        LD   *AR6-0,-4,B         ;rightbottom point
        OR   B,4,A
        LD   *AR6-0,-4,B         ;right point
        OR   B,5,A
        LD   *AR6-,-4,B          ;righttop point
        OR   B,6,A
        LD   *AR6+0,-4,B         ;top point 
        OR   B,7,A
        RET

*****************************************************************************
* fill image bound edge width black
*****************************************************************************
        .text
BoundFill:
        STM  #IMAGEADDR-IMAGEOFF,AR6
        STM  #STARTROW-1,BRC
        RPTB FillTop-1
        RPT  #IMAGEWIDTH-1
        ST   #BLACK,*AR6+
FillTop
        STM  #IMAGEADDR-IMAGEOFF+ENDOFROW*IMAGEWIDTH,AR6
        STM  #IMAGEHEIGHT-ENDOFROW-1,BRC
        RPTB FillBottom-1
        RPT  #IMAGEWIDTH-1
        ST   #BLACK,*AR6+
FillBottom
        STM  #IMAGEADDR-IMAGEOFF+STARTROW*IMAGEWIDTH,AR6
        STM  #ENDOFROW-STARTROW-1,BRC
        RPTB #FillLeft-1
        RPT  #STARTCOL-1
        ST   #BLACK,*AR6+
        MAR  *+AR6(ENDOFCOL-STARTCOL)
        RPT  #IMAGEWIDTH-ENDOFCOL-1
        ST   #BLACK,*AR6+
FillLeft
        RET

*****************************************************************************
* calculate tan value's angle,that is theta=atan(y.x)
*****************************************************************************
        .text
CalAngle:
        SSBX  SXM
        PSHM  AR2
        PSHM  AR4
        ST    #0,Theta         ;set theta=0
        STM   #DeltY,AR2
        STM   #DeltX,AR4
        DLD   *AR4,A           ;DeltX==0?
        BC    $5,ANEQ
        DLD   *AR2,B           ;DeltY==0?
        BC    Exit_Cal,BEQ     ;Theta=0
$5
        ABS   A                ;|DeltX|
        SUB   #1,A,B
        BC    No_Special,BGEQ  ;|DeltX|<1?
        DLD   *AR2,B           ;DeltY==0?
        BC    Exit_Cal,BEQ     ;theta=0
        ST    #45,Theta
        BC    Exit_Cal,BGT     ;theta=45
        ST    #135,Theta       ;theta=135
        B     Exit_Cal
No_Special
        DLD   *AR2,B           ;DeltY==0?
        ABS   B                ;|DeltY|
        SFTA  A,-8,A           ;|DeltX|/256
        RSBX  SXM
        CALL  DivideU32
        CALL  GetTheta         ;search the theta
        SSBX  SXM
        NOP
        DLD   *AR4,A           ;DeltX
        BC    $1,AGEQ
        DLD   *AR2,A           ;DeltY
        BC    Angle_Common,ALT ;DeltX<0 && DelyY<0  --3
        NEG   B                ;DeltX<0 && DeltY>=0 --2
        B     Angle_Common
$1
        DLD   *AR2,A           ;DeltY
        BC    $2,ALT
        ADD   #180,B           ;DeltX>0 && DeltY>=0 --1
        B     Angle_Common
$2
        NEG   B                ;DeltX>0 && DeltY<0  --4
        ADD   #180,B
Angle_Common
        SFTA  B,-1,B           ;Theta/=2
        BC    $3,BGEQ
        ADD   #180,B
$3
        STL   B,Theta          ;Theta=CalAngle(DeltY/DeltX)
Exit_Cal
        RSBX  SXM
        POPM  AR4
        POPM  AR2
        RET

*****************************************************************************
* reclaculate the smoothed block dircetion
*****************************************************************************
        .text
ReCalAngle:
        PSHM  AR2
        PSHM  AR4
        ST    #0,Theta         ;set theta=0
        STM   #DeltY,AR2
        STM   #DeltX,AR4
        DLD   *AR4,A           ;DeltX==0?
        BC    $5,ANEQ
        DLD   *AR2,B           ;DeltY==0?
        BC    Exit_ReCal,BEQ   ;Theta=0
$5
        ABS   A                ;|DeltX|
        SUB   #30,A,B
        BC    NoRe_Special,BGEQ;|DeltX|<0.001?
        DLD   *AR2,B           ;DeltY==0?
        BC    Exit_ReCal,BEQ   ;theta=0
        ST    #45,Theta
        BC    Exit_ReCal,BGT   ;theta=45
        ST    #135,Theta       ;theta=135
        B     Exit_ReCal
NoRe_Special
        DLD   *AR2,B           ;DeltY==0?
        ABS   B                ;|DeltY|
        SFTA  A,-8,A           ;|DeltX|/256
        RSBX  SXM
        CALL  DivideU32
        CALL  GetTheta         ;search the theta
        SSBX  SXM
        NOP
        DLD   *AR4,A              ;DeltX
        BC    $1,ALEQ
        DLD   *AR2,A              ;DeltY
        BC    ReAngle_Common,AGEQ ;DeltX>0 && DelyY>=0 --1
        NEG   B                   ;DeltX>0 && DeltY<0  --4
        B     ReAngle_Common
$1
        DLD   *AR2,A              ;DeltY
        BC    $2,AGEQ
        ADD   #180,B              ;DeltX<=0 && DeltY<0 --3
        B     ReAngle_Common
$2
        NEG   B                   ;DeltX<=0 && DeltY>=0--2
        ADD   #180,B
ReAngle_Common
        SFTA  B,-1,B              ;Theta/=2
        BC    $3,BGEQ
        ADD   #180,B
$3
        STL   B,Theta             ;Theta=CalAngle(DeltY/DeltX)
Exit_ReCal
        SSBX  SXM
        POPM  AR4
        POPM  AR2
        RET

*****************************************************************************
*Tangle calculate image block direction
*****************************************************************************
        .text
TangleValue:
        PSHM  AR2
        PSHM  AR4
        ST    #180,Theta       ;set theta=180
        STM   #DeltY,AR2
        STM   #DeltX,AR4
        DLD   *AR4,A           ;DeltX==0?
        BC    $5,ANEQ
        DLD   *AR2,B           ;DeltY==0?
        BC    Exit_TaCal,BEQ   ;Theta=0
        BC    $6,BGT           ;DeltY>0
        ST    #270,Theta       ;Theta=270
        B     Exit_TaCal
$6                             ;DeltY<0
        ST    #90,Theta        ;Theta=90
        B     Exit_TaCal
$5                             
        BC    $7,AGT           ;DeltX>0
        DLD   *AR2,B 
        BC    Exit_TaCal,BEQ   ;DeltY==0?
        B     No_TaSpecial
$7
        DLD   *AR2,B           ;DeltY==0?
        BC    No_TaSpecial,BNEQ
        ST    #0,Theta
        B     Exit_TaCal
No_TaSpecial
        DLD   *AR2,B           ;DeltY==0?
        ABS   B                ;|DeltY|
        SFTA  B,8,B            ;|DeltY|*256
        DLD   *AR4,A
        ABS   A                ;|DeltX|
        RSBX  SXM
        CALL  DivideU32
        CALL  GetTheta         ;search the theta
        SSBX  SXM
        NOP
        DLD   *AR4,A              ;DeltX
        BC    $1,ALEQ
        DLD   *AR2,A              ;DeltY
        BC    TaAngle_Common,AGEQ ;DeltX>0 && DelyY>=0 --1
        NEG   B                   ;DeltX>0 && DeltY<0  --4
        ADD   #360,B
        B     TaAngle_Common
$1
        DLD   *AR2,A              ;DeltY
        BC    $2,AGEQ
        ADD   #180,B              ;DeltX<=0 && DeltY<0 --3
        B     TaAngle_Common
$2
        NEG   B                   ;DeltX<=0 && DeltY>=0--2
        ADD   #180,B
TaAngle_Common
        STL   B,Theta             ;Theta=CalAngle(DeltY/DeltX)
Exit_TaCal
        SSBX  SXM
        POPM  AR4
        POPM  AR2
        RET

*****************************************************************************
* unsigned 32 bit divide function defines here
* Divisor=B
*****************************************************************************
        .text
DivideU32:
        PSHM  AR1
        PSHM  AR3
        PSHM  AR5
        PSHM  AR7
        STM   #Dividen,AR1
        STM   #Quoten,AR3
        STM   #Remain,AR5
        STM   #31,AR7
        RSBX  C
        DST   B,*AR1        ;Dividen
        ROL   B             ;C=maxbit(Dividend)
        DST   B,*AR5        ;Remain
        ROL   B             
        AND   #01h,B
        DST   B,*AR1        ;Dividend
        SUB   B,B
        DST   B,*AR3        ;Quoten reset to zero
Divide_Loop
        DLD   *AR1,B
        SUB   A,B           ;compare subtract
        BC    Divide_Great,BGEQ
        DLD   *AR1,B        ;Dividend
        ST    #0,Sqrt_temp  ;Sqrt_temp=0
        B     Divide_Inc    ;Quotent
Divide_Great
        DST   B,*AR1        ;Dividend
        ST    #1,Sqrt_temp  ;B=1
Divide_Inc
        DLD   *AR3,B        ;Quotent
        SFTA  B,1,B         ;Quotent<<1
        ADD   Sqrt_temp,B   ;Quotent+Sqrt_temp
        DST   B,*AR3        ;Quotent
        RSBX  C
        DLD   *AR5,B        ;Remain
        ROL   B             ;Remaind<<1
        DST   B,*AR5        ;Remaind

        DLD   *AR1,B        ;Dividend
        ROL   B             ;Dividend<<1+C
        DST   B,*AR1        ;Dividend

        BANZ  Divide_Loop,*AR7-
        DLD   *AR3,B        ;Quoten
        POPM  AR7
        POPM  AR5
        POPM  AR3
        POPM  AR1
        RET
*****************************************************************************
* search the table find the theta of atan(dy/dx)
*****************************************************************************
        .text
GetTheta:
        PSHM  AR0
        PSHM  AR1
        STM   #AtanTable+89,AR1
        STM   #89,AR0
Find_Loop
        LD    *AR1-,A
        SUB   B,A
        BC    Find_Theta,ALEQ
        BANZ  Find_Loop,*AR0-
        STM   #0,AR0
Find_Theta
        LDM   AR0,B
        POPM  AR1
        POPM  AR0
        RET
*****************************************************************************
* search the table find the sin value of theta,result=A
*****************************************************************************
        .text
GetSinValue:
       CALL  CheckValue 
       NEG   A
       ADD   #CosTable+90,A
       PSHM  AR6
       STLM  A,AR6
       NOP
       NOP
       LD    *AR6,16,A
       POPM  AR6
       RET

*****************************************************************************
* search the table fine the cos value of theta,result=A 
*****************************************************************************
        .text
GetCosValue:
       CALL  CheckValue 
       ADD   #CosTable,A
       PSHM  AR6
       STLM  A,AR6
       NOP
       NOP
       LD    *AR6,16,A
       POPM  AR6
       RET

*****************************************************************************
* search the table fine the theta position
*****************************************************************************
        .text
CheckValue:
       ST    #1,Minus
       LD    Theta,A
       SUB   #90,A,B
       RC    BLT
       SUB   #180,A,B
       BC    Great_180,BGT
       ST    #2,Minus
       LD    #180,A
       SUB   Theta,A
       RET
Great_180
       SUB   #270,A,B
       BC    Great_270,BGT
       ST    #3,Minus
       SUB   #180,A
       RET
Great_270
       ST    #4,Minus
       LD    #360,A
       SUB   Theta,A
       RET 

*****************************************************************************
* check the different theta value
; Ix=AR5,Iy=AR6,Theta3,Theta1
*****************************************************************************
		.text
Check_Theta:
		MAR   *+AR5(3)
		MAR   *+AR6(3)
		LD    *AR5,A
		ADD   #360,A
		SUB   *AR6,A
		RPT   #15
		SUBC  Divisor,A
		STH   A,Theta3
        LD    Theta3,A
        ADD   #360,A
		SUB   Theta1,A
		RPT   #15
		SUBC  Divisor,A
		STH   A,Theta3
    	LD    Theta3,A
		SUB   #180,A,B
		BC    $9,BLEQ
		NEG   A
		ADD   #360,A
		STL   A,Theta3
$9
		MAR   *+AR5(-3)
		MAR   *+AR6(-3)
		RET

*****************************************************************************
* input theta and distancex,distancey,calculate the coordinate
*****************************************************************************
		.text
Cal_Coordinate:
		PSHM  AR4
		STM   DeltY,AR4
		LD    DistanceY,A
		DST   A,*AR4
		LD    SinValue,B
		CALL  MultipyValue
		SFTA  B,-15,B        
		STL   B,CoordX      ;OffsetY=DistanceY*SinValue 
		STM   DeltX,AR4
	    LD    DistanceX,A  
		DST   A,*AR4
		LD    CosValue,B
		CALL  MultipyValue
		SFTA  B,-15,B       ;OffsetX=DistanceX*CosValue
		SUB   CoordX,B     
		STL   B,CoordX      ;CoordinateX=OffsetX-OffsetY
		
		LD    DistanceX,A
		LD    SinValue,B
		CALL  MultipyValue
		SFTA  B,-15,B
        STL   B,CoordY      ;OffsetX=DistanceX*SinValue
		LD    DistanceY,A
		LD    CosValue,B
		CALL  MultipyValue
		SFTA  B,-15,B       ;OffsetY=DistanceY*CosValue 
		ADD   CoordY,B
		STL   B,CoordY      ;CoordinateY=OffsetX+OffsetY
		POPM  AR4
		RET

*****************************************************************************
* input ix,ix,jx,thetax,calculate distance
; AR6=Ix,AR1=Ix,AR7=Jx,CosValue,SinValue
*****************************************************************************
		.text
Cal_Distance:
		SSBX  TC
		LD    *AR6+,A
		SUB   *AR1+,A
		STL   A,DistanceX
		LD    *AR6+,A
		SUB   *AR1+,A
		STL   A,DistanceY
		CALL  Cal_Coordinate
        LD    *AR7+,A
		ADD   CoordX,A
		STL   A,CoordX
		RC    ALEQ
		SUB   #IMAGEWIDTH,A
		RC    AGEQ
        LD    *AR7+,A
		ADD   CoordY,A
		STL   A,CoordY
		RC    ALEQ
		SUB   #IMAGEHEIGHT,A
		RC    AGEQ
		SQUR  DistanceX,A
		SQUR  DistanceY,B
		ADD   A,B
		SFTA  B,-8,B
		ADD   #9,B
		RSBX  TC
		RET

;----------------------------------------------------------------------------
       .end

